package com.gitee.wsl.mathematics.matrix.mat4.ext

import com.gitee.wsl.mathematics.complex.quaternion.Quaternion
import com.gitee.wsl.mathematics.matrix.mat4.Matrix4
import com.gitee.wsl.mathematics.matrix.mat4.MutableMatrix4
import com.gitee.wsl.mathematics.number.compareTo
import com.gitee.wsl.mathematics.vector.vec3.Vector3
import com.gitee.wsl.unit.angle.AngleUnit

val <T:Number> Matrix4<T, *, *>.rotationMat3
    get() = createMat3 { result->
    result[0, 0] = this[0, 0]
    result[0, 1] = this[0, 1]
    result[0, 2] = this[0, 2]

    result[1, 0] = this[1, 0]
    result[1, 1] = this[1, 1]
    result[1, 2] = this[1, 2]

    result[2, 0] = this[2, 0]
    result[2, 1] = this[2, 1]
    result[2, 2] = this[2, 2]
}

val <T:Number> Matrix4<T, *, *>.rotationTransposed
    get() = createMat3 { result->
    result[0, 0] = this[0, 0]
    result[0, 1] = this[1, 0]
    result[0, 2] = this[2, 0]

    result[1, 0] = this[0, 1]
    result[1, 1] = this[1, 1]
    result[1, 2] = this[2, 1]

    result[2, 0] = this[0, 2]
    result[2, 1] = this[1, 2]
    result[2, 2] = this[2, 2]
}

/**
 * Get rotation from this matrix.
 *  Rotation quaternion as an output parameter
 */
val <T:Number> Matrix4<T, *, *>.rotationVec4
    get() = createVec4{result->

    val trace = this[0, 0] + this[1, 1] + this[2, 2]

    if (trace > zero) {
        var s = sqrt(trace + 1f)
        result.w = s * 0.5f
        s = 0.5f / s

        result.x = (this[2, 1] - this[1, 2]) * s
        result.y = (this[0, 2] - this[2, 0]) * s
        result.z = (this[1, 0] - this[0, 1]) * s
    } else {
        val i =
            if (this[0, 0] < this[1, 1]) {
                if (this[1, 1] < this[2, 2]) {
                    2
                } else {
                    1
                }
            } else {
                if (this[0, 0] < this[2, 2]) {
                    2
                } else {
                    0
                }
            }
        val j = (i + 1) % 3
        val k = (i + 2) % 3

        var s = sqrt(this[i, i] - this[j, j] - this[k, k] + 1f)
        result.proxy[i] = s * 0.5f
        s = 0.5f / s

        result.w = (this[k, j] - this[j, k]) * s
        result.proxy[j] = (this[j, i] + this[i, j]) * s
        result.proxy[k] = (this[k, i] + this[i, k]) * s
    }
}


fun<T:Number> MutableMatrix4<T, *, *>.setToRotation(axis: Vector3<T,*>, angle: AngleUnit) =
    setToRotation(axis.x, axis.y, axis.z, angle)

fun<T:Number> MutableMatrix4<T, *, *>.setToRotation(ax: T, ay: T, az: T, angle: AngleUnit) {
    val a = angle.radian + zero
    var x = ax
    var y = ay
    var z = az
    m30 = zero
    m31 = zero
    m32 = zero
    m03 = zero
    m13 = zero
    m23 = zero
    m33 = one
    val s = sin(a)
    val c = cos(a)
    if (x > 0f && y == 0f && z == 0f) {
        m11 = c
        m22 = c
        m21 = s
        m12 = -s
        m10 = zero
        m20 = zero
        m01 = zero
        m02 = zero
        m00 = one
    } else if (x == 0f && y > 0f && z == 0f) {
        m00 = c
        m22 = c
        m02 = s
        m20 = -s
        m10 = zero
        m01 = zero
        m21 = zero
        m12 = zero
        m11 = one
    } else if (x == 0f && y == 0f && z > 0f) {
        m00 = c
        m11 = c
        m10 = s
        m01 = -s
        m20 = zero
        m21 = zero
        m02 = zero
        m12 = zero
        m22 = one
    } else {
        val recipLen = 1f / sqrt(x * x + y * y + z * z)
        x *= recipLen
        y *= recipLen
        z *= recipLen

        val nc = 1f - c
        val xy = x * y
        val yz = y * z
        val zx = z * x
        val xs = x * s
        val ys = y * s
        val zs = z * s
        m00 = x * x * nc + c
        m01 = xy * nc - zs
        m02 = zx * nc + ys
        m10 = xy * nc + zs
        m11 = y * y * nc + c
        m12 = yz * nc - xs
        m20 = zx * nc - ys
        m21 = yz * nc + xs
        m22 = z * z * nc + c
    }
}

fun<T:Number> MutableMatrix4<T, *, *>.setToRotation(quaternion: Quaternion<T,*>){
    val r = quaternion.w
    val i = quaternion.x
    val j = quaternion.y
    val k = quaternion.z

    var s = sqrt(r * r + i * i + j * j + k * k)
    s = 1f / (s * s)

    this[0, 0] = 1 - 2 * s * (j * j + k * k)
    this[0, 1] = 2 * s * (i * j - k * r)
    this[0, 2] = 2 * s * (i * k + j * r)
    this[0, 3] = zero

    this[1, 0] = 2 * s * (i * j + k * r)
    this[1, 1] = 1 - 2 * s * (i * i + k * k)
    this[1, 2] = 2 * s * (j * k - i * r)
    this[1, 3] = zero

    this[2, 0] = 2 * s * (i * k - j * r)
    this[2, 1] = 2 * s * (j * k + i * r)
    this[2, 2] = 1 - 2 * s * (i * i + j * j)
    this[2, 3] = zero

    this[3, 0] = zero
    this[3, 1] = zero
    this[3, 2] = zero
    this[3, 3] = one
}

/**
 * Sets this matrix to a rotation matrix from the given euler angles.
 *
 * @param yaw the yaw
 * @param pitch the pitch
 * @param roll the roll
 * @return this matrix
 */
fun<T:Number> MutableMatrix4<T, *, *>.setFromEulerAngleUnits(yaw: AngleUnit, pitch: AngleUnit, roll: AngleUnit) {
    val a = yaw.radian+zero
    val b = pitch.radian+zero
    val c = roll.radian+zero

    val ci = cos(a)
    val cj = cos(b)
    val ch = cos(c)
    val si = sin(a)
    val sj = sin(b)
    val sh = sin(c)
    val cc = ci * ch
    val cs = ci * sh
    val sc = si * ch
    val ss = si * sh

    m00 = cj * ch
    m01 = sj * sc - cs
    m02 = sj * cc + ss
    m03 = zero

    m10 = cj * sh
    m11 = sj * ss + cc
    m12 = sj * cs - sc
    m13 = zero

    m20 = -sj
    m21 = cj * si
    m22 = cj * ci
    m23 = zero

    m30 = zero
    m31 = zero
    m32 = zero
    m33 = one

}